﻿package physics
{
import org.papervision3d.core.geom.TriangleMesh3D
import org.papervision3d.core.math.Matrix3D
import org.papervision3d.core.math.Number3D
import physics.Number3DPlus
/**
 * @author Muzer (RigidBody.as)
 * @modifier Exey Panteleev
 */
public class PolygonPhysics 
{
    public var Object3D:TriangleMesh3D;
	public var Mass:Number;
	public var CoeffOfFriction:Number;
	public var CoeffOfRestitution:Number;
	
	public var CGPosition:Number3D;
	public var Forces:Number3D;
	public var Torques:Number3D;
	public var LinVelocity:Number3D;
	public var RotVelocity:Number3D;
	public var OBBSize:Number3D;
	
	public var VerticesPos:Array;
	public var localAxis:Array;
	
	private var Angle:Number;
	private var Axis:Number3D;
	public var LinImpulse:Number3D;
	public var RotImpulse:Number3D;
	private var LinAcceleration:Number3D;
	private var RotAcceleration:Number3D;
	
	private var RotationMatrix:Matrix3D;
	
	public var isDetained:Boolean = false;
	
	private var tempVerticesPos:Array = new Array();
	
	public var isStopped:Boolean = false
	
	public function PolygonPhysics(obj:TriangleMesh3D,mass:Number=10,friction:Number=0.5,restitution:Number=1) 
	{
		Object3D = obj;
		Mass=mass;
		CoeffOfFriction=friction;
		CoeffOfRestitution = restitution;
		
		CGPosition=new Number3D(obj.x,obj.y,obj.z);
		Forces=new Number3D(0,0,0);
		Torques=new Number3D(0,0,0);
		LinVelocity=new Number3D(0,0,0);
		RotVelocity=new Number3D(0,0,0);
		Axis=new Number3D(0,1,0);
		LinImpulse=new Number3D(0,0,0);
		RotImpulse=new Number3D(0,0,0);
		LinAcceleration=new Number3D(0,0,0);
		RotAcceleration = new Number3D(0, 0, 0);
		
		VerticesPos = new Array();
		localAxis=new Array(new Number3D(1,0,0),
							new Number3D(0,1,0),
							new Number3D(0, 0, 1));
							
		// store vertices positions for reset 
		for (var i:int = 0; i < Object3D.geometry.vertices.length; i++) 
		{
			tempVerticesPos[i] = new Number3D(
							Object3D.geometry.vertices[i].x,
							Object3D.geometry.vertices[i].y,
							Object3D.geometry.vertices[i].z);
		}
	}
	
	public function setPos(pos:Number3D):void
	{
		CGPosition=pos;
		Object3D.x=pos.x;
		Object3D.y=pos.y;
		Object3D.z=pos.z;
	}
	
	public function AddTorque(t:Number3D):void
	{
		Torques=Number3D.add(Torques,t);
		RotAcceleration=Torques.clone();
	}
	
	public function AddForce(p:Number3D,f:Number3D):void
	{
		Forces=Number3D.add(Forces,f);
		LinAcceleration=Number3DPlus.divide(Forces,Mass);
		AddTorque(Number3D.cross(f,Number3D.sub(p,CGPosition)));
	}
	
	
	public function ApplyImpulse():void
	{
		LinVelocity=Number3D.add(LinVelocity,Number3DPlus.divide(LinImpulse,Mass));
		
		RotVelocity=Number3D.add(RotVelocity,RotImpulse);
		
		LinImpulse=Number3D.ZERO;
		RotImpulse=Number3D.ZERO;
	}
	
	private function RotationRB():void
	{
		RotationMatrix=Matrix3D.rotationMatrix(Axis.x,Axis.y,Axis.z,Angle);
		Object3D.transformVertices(RotationMatrix);
		updateLocalAxis(RotationMatrix);
	}
	
	private function updateLocalAxis(rotation:Matrix3D):void
	{
		for(var i:String in localAxis)
		{
			Matrix3D.multiplyVector(rotation,localAxis[i]);
			localAxis[i].normalize();
		}
	}
	
	private function updateVerticesPos():void
	{
		var vertices:Array=new Array();
		
		for(var i:String in Object3D.geometry.vertices)
		{
			vertices[i]=new Number3D(Object3D.geometry.vertices[i].x,Object3D.geometry.vertices[i].y,Object3D.geometry.vertices[i].z);
			VerticesPos[i]=Number3D.add(vertices[i],CGPosition);
		}
	}
	
	
	public function resetPos():void 
	{
		for (var i:int = 0; i < Object3D.geometry.vertices.length; i++) 
		{
				Object3D.geometry.vertices[i].x = tempVerticesPos[i].x
				Object3D.geometry.vertices[i].y = tempVerticesPos[i].y
				Object3D.geometry.vertices[i].z = tempVerticesPos[i].z
		}
	}
	
	public function ResetImpulse():void
	{
		LinImpulse=Number3D.ZERO;
		RotImpulse=Number3D.ZERO;
	}
	
	public function ResetForce():void
	{
		Forces            = Number3D.ZERO;
		Torques            = Number3D.ZERO;
		LinAcceleration    = Number3D.ZERO;
		RotAcceleration    = Number3D.ZERO;
	}
	
	public function Update(dt:Number):void
	{
		ApplyImpulse();
		
		CGPosition=Number3D.add(CGPosition,Number3D.add(Number3DPlus.multiply(LinVelocity,dt),Number3DPlus.multiply(LinAcceleration,dt*dt*0.5)));
		Object3D.x=CGPosition.x;
		Object3D.y=CGPosition.y;
		Object3D.z=CGPosition.z;
		
		LinVelocity=Number3D.add(LinVelocity,Number3DPlus.multiply(LinAcceleration,dt));
		
		Axis=RotVelocity.clone();
		Axis.normalize();
		
		var temp:Number3D=Number3D.add(Number3DPlus.multiply(RotVelocity,dt),Number3DPlus.multiply(RotAcceleration,dt*dt*0.5));
		Angle=temp.modulo;
		
		RotationRB();
		
		RotVelocity=Number3D.add(RotVelocity,Number3DPlus.multiply(RotAcceleration,dt));
		
		LinVelocity=Number3DPlus.multiply(LinVelocity,0.995);
		RotVelocity=Number3DPlus.multiply(RotVelocity,0.995);
		
		updateVerticesPos();
	}
}
}